#include "physics.h"
#include "qmath.h"

Physics::Physics()
{
    mass = 1;
    cSpringFactor = 5;
    kEnvironmentFactor = 0.1;
    innerT = 0.0;

    v = 0;
    a = 0;
    delta = 0.025;
    xPrev = 0;
    xCurr = xPrev + v * delta;
    diffCurr = differential(delta+0.35);

    wA = wFi = wOmega = 0;
    hA = hFi = hOmega = 0;
}

double Physics::exact(double t, double k)
{
    double d = exp(-0.5*(sqrt(pow(k, 2.0)-4+k)*t));
    double d2 = exp(0.5*(sqrt(pow(k, 2.0)-4-k)*t));
    return d + d2;
}

void Physics::restart(double x0, double v0, double deltai)
{
    double F0 = cSpringFactor * (w(0) - x0) - kEnvironmentFactor * v0 + h(0);
    xPrev = x0;
    v = v0;
    delta = deltai;
    xCurr = xPrev + v * delta;// + F0 * delta * delta / (mass * 2.0);
    diffCurr = differential(delta+0.35);
    innerT = 0.0;
    a = 0;
}

void Physics::update(double t)
{
    while(innerT <= t)
    {
        double val = (2.0 * delta * delta * (cSpringFactor * (w(innerT) - xCurr) + h(innerT)) + 4.0 * mass * xCurr
                      + xPrev * (delta * kEnvironmentFactor - 2.0 * mass)) / (2.0 * mass + delta * kEnvironmentFactor);

        v = velocity(xPrev, val);
        a = acceleration(xPrev, xCurr, val);
        xPrev = xCurr;
        xCurr = val;

        diffCurr = differential(innerT + 0.35);

        innerT += delta;
    }
}

double Physics::x(double t1, double xt0, double xt1)
{
    return (2.0 * delta * delta * (cSpringFactor * (w(t1) - xt1) + h(t1))
            + 4 * mass * xt1 - (2 * mass - delta * kEnvironmentFactor) * xt0) /
            (2 * mass + delta * kEnvironmentFactor);
}

double Physics::x(double t1, double xt0, double xt1, double vv)
{
    return (delta * delta * (cSpringFactor * (w(t1) - xt1)
                             - kEnvironmentFactor * vv + h(t1))
            - mass * (xt0 - 2 * xt1)) / mass;
}

double Physics::velocity(double xt0, double xt2)
{
    return (xt2 - xt0) / (2 * delta);
}

double Physics::acceleration(double xt0, double xt1, double xt2)
{
    return (xt2 - 2.0 * xt1 + xt0) / (delta * delta);
}

double Physics::f(double t)
{
    return cSpringFactor * (w(t) - xCurr);
}

double Physics::g(double t)
{
    return - kEnvironmentFactor * v;
}

double Physics::h(double t)
{
    double val = hA;
    switch (hOption)
    {
        case 1:
            val = hA * qSin(hOmega * t + hFi);
            if(val > 0)
                val = 1;
            if (val < 0)
                val = -1;
            break;
        case 2:
            val = hA * qSin(hOmega * t + hFi);
            break;
    }
    return val;
}

double Physics::w(double t)
{
    double val = wA;
    switch (wOption)
    {
        case 1:
            val = wA * qSin(wOmega * t + wFi);
            if(val > 0)
                val = 1;
            if (val < 0)
                val = -1;
            break;
        case 2:
            val = wA * qSin(wOmega * t + wFi);
            break;
    }
    return val;
}

double Physics::differential(double t)
{
    double s = kEnvironmentFactor * kEnvironmentFactor - 4.0 * cSpringFactor * mass;
    double res;
    if (s >= 0)
    {
        double sq = sqrt(s);
        double F1 = exp(-t * (sq + kEnvironmentFactor) /(2.0 * mass));
        double F2 = exp(t * (sq - kEnvironmentFactor) / (2.0 * mass));
        res = F1 + F2;
    }
    else
    {
       s = fabs(s);
       double sq = sqrt(s);
       double fi = t * sq / (2.0 * mass);
       double o = t * kEnvironmentFactor / (2.0 * mass);
       res = (cos(fi) + sin(fi)) / exp(o);
    }
    return res;
}
